# -*- coding: utf-8 -*-
from copy import deepcopy
import logging
import trimesh
from mayavi import mlab
import numpy as np
from numpy.linalg import inv, norm
import time

from autolab_core import RigidTransform
from .contacts import Contact3D
from .quality import PointGraspMetrics3D
import trimesh.transformations as tf
from scipy.linalg import *
from .grasp import PointGrasp


def change_ray_color(ray, color):
    colors = np.ones((len(ray.entities), 3))
    colors_1 = (colors * color).astype(np.uint8)  # approach
    ray.colors = colors_1

class ApproachParallelJawPtGrasp3D(PointGrasp):
    """ Parallel Jaw point grasps in 3D space.
    """

    def __init__(self, configuration, frame='object', grasp_id=None):
        # get parameters from configuration array

        grasp_center, grasp_axis, grasp_width, \
        grasp_approach, jaw_width, grasp_score, \
        distance, grasp_point,  \
        offset_distance, grasp_width_new, min_grasp_width = \
            ApproachParallelJawPtGrasp3D.approach_params_from_configuration(configuration)
        self.grasp_approach = grasp_approach
        self.approach_distance = distance
        self.grasp_point = grasp_point

        self.center_ = grasp_center
        self.axis_ = grasp_axis / np.linalg.norm(grasp_axis)
        self.max_grasp_width_ = grasp_width
        self.jaw_width_ = jaw_width
        self.real_jaw_width_ = self.jaw_width_
        self.min_grasp_width_ = min_grasp_width
        self.grasp_score = grasp_score
        self.frame_ = frame
        self.grasp_id_ = grasp_id
        self.offset_distance = offset_distance

        self.grasp_width_new = grasp_width_new #+ (self.real_jaw_width_ -self.jaw_width_)
        self.center_offset = self.center_ + self.offset_distance * self.axis_

    @property
    def center(self):
        """ :obj:`numpy.ndarray` : 3-vector specifying the center of the jaws """
        return self.center_

    @center.setter
    def center(self, x):
        self.center_ = x

    @property
    def axis(self):
        """ :obj:`numpy.ndarray` : normalized 3-vector specifying the line between the jaws """
        return self.axis_

    @property
    def open_width(self):
        """ float : maximum opening width of the jaws """
        return self.max_grasp_width_

    @property
    def close_width(self):
        """ float : minimum opening width of the jaws """
        return self.min_grasp_width_

    @property
    def jaw_width(self):
        """ float : width of the jaws in the tangent plane to the grasp axis """
        return self.jaw_width_

    @property
    def real_jaw_width(self):
        """ float : width of the jaws in the tangent plane to the grasp axis """
        return self.real_jaw_width_

    @property
    def approach_angle(self):
        """ float : approach angle of the grasp """
        return self.approach_angle_

    @property
    def frame(self):
        """ :obj:`str` : name of grasp reference frame """
        return self.frame_

    @property
    def id(self):
        """ int : id of grasp """
        return self.grasp_id_

    @frame.setter
    def frame(self, f):
        self.frame_ = f

    @approach_angle.setter
    def approach_angle(self, angle):
        """ Set the grasp approach angle """
        self.approach_angle_ = angle

    @property
    def endpoints(self):
        """
        Returns
        -------
        :obj:`numpy.ndarray`
            location of jaws in 3D space at max opening width """
        return self.center_ - (self.max_grasp_width_ / 2.0) * self.axis_, self.center_ + (
                    self.max_grasp_width_ / 2.0) * self.axis_,

    @property
    def contactpoints(self):
        return self.center_ - (self.jaw_width_ / 2.0) * self.axis_, self.center_ + (self.jaw_width_ / 2.0) * self.axis_

    @property
    def virtual_contactpoints(self):
        return self.center_ - (self.real_jaw_width_ / 2.0) * self.axis_, self.center_ + (
                    self.real_jaw_width_ / 2.0) * self.axis_

    @property
    def virtual_contactpoints_offset(self):
        # print('extend pad ****************', self.real_jaw_width_ - self.jaw_width, 'extend pad ****************')
        extend_pad = self.real_jaw_width_ - self.jaw_width
        self.grasp_width_new_ = self.grasp_width_new + extend_pad
        return self.center_offset - (self.grasp_width_new_  / 2.0) * self.axis_, self.center_offset + (
                    self.grasp_width_new_  / 2.0) * self.axis_

    @staticmethod
    def distance(g1, g2, alpha=0.03, beta=0.045):
        """ Evaluates the distance between two grasps.

        Parameters
        ----------
        g1 : :obj:`ApproachParallelJawPtGrasp3D`
            the first grasp to use
        g2 : :obj:`ApproachParallelJawPtGrasp3D`
            the second grasp to use
        alpha : float
            parameter weighting rotational versus spatial distance

        Returns
        -------
        float
            distance between grasps g1 and g2
        """
        center_dist = np.linalg.norm(g1.center - g2.center)
        value = np.abs(g1.axis.dot(g2.axis))
        value_1 = g1.approached_full_axis[:, 0].dot(g2.approached_full_axis[:, 0])
        if value > 1:
            value = 1
        if value_1 > 1:
            value_1 = 1
        if value_1 < -1:
            value_1 = -1

        axis_dist = (1.0 / np.pi) * np.arccos(value)
        approach_dist = (1.0 / np.pi) * np.arccos(value_1)
        # approach_dist = 0
        return center_dist * 2.0 + alpha * axis_dist + beta * approach_dist

    @staticmethod
    def approach_configuration_from_params(center, axis, width, grasp_approach=[0., 0., 0.], jaw_width=0,
                                           grasp_score=[0., 0., 0.], distance=0.0, grasp_point=[0, 0, 0],
                                           offset_distance=0.0, grasp_width_new=0.0,min_width=0):
        """ Converts grasp parameters to a configuration vector. """
        if np.abs(np.linalg.norm(axis) - 1.0) > 1e-5:
            raise ValueError('Illegal grasp axis. Must be norm one')
        if grasp_width_new == 0:
            raise ValueError('Illegal grasp_width_new, NOT ZERO')
        configuration = np.zeros(21)
        configuration[0:3] = center
        configuration[3:6] = axis
        configuration[6] = width
        configuration[7:10] = grasp_approach
        configuration[10] = jaw_width
        configuration[11:14] = grasp_score
        configuration[14] = distance  # d is approach distance
        configuration[15:18] = grasp_point
        configuration[18] = offset_distance
        configuration[19] = grasp_width_new
        configuration[20] = min_width

        return configuration

    @staticmethod
    def show_line(ori, target, color='g', scale_factor=0.0002):
        if color == 'b':
            color_f = (0, 0, 1)
        elif color == 'r':
            color_f = (1, 0, 0)
        elif color == 'g':
            color_f = (0, 1, 0)
        else:
            color_f = (1, 1, 1)
        mlab.plot3d([ori[0], target[0]], [ori[1], target[1]], [ori[2], target[2]], color=color_f,
                    tube_radius=scale_factor)

    @staticmethod
    def approach_params_from_configuration(configuration):
        """ Converts configuration vector into grasp parameters.

        Returns
        -------
        grasp_center : :obj:`numpy.ndarray`
            center of grasp in 3D space
        grasp_axis : :obj:`numpy.ndarray`
            normalized axis of grasp in 3D space
        max_width : float
            maximum opening width of jaws
        angle : float
            approach angle
        jaw_width : float
            width of jaws
        min_width : float
            minimum closing width of jaws
        """
        if not isinstance(configuration, np.ndarray) or (configuration.shape[0] != 21):
            raise ValueError('Configuration must be numpy ndarray of size 19')
        offset_distance = configuration[18]
        grasp_width_new = configuration[19]
        min_grasp_width = configuration[20]

        if np.abs(np.linalg.norm(configuration[3:6]) - 1.0) > 1e-5:
            raise ValueError('Illegal grasp axis. Must be norm one')
        return configuration[0:3], configuration[3:6], configuration[6], configuration[7:10], configuration[
            10], configuration[11:14], configuration[14], configuration[15:18],  offset_distance, grasp_width_new, min_grasp_width

    @staticmethod
    def center_from_endpoints(g1, g2):
        """ Grasp center from endpoints as np 3-arrays """
        grasp_center = (g1 + g2) / 2
        return grasp_center

    @staticmethod
    def axis_from_endpoints(g1, g2):
        """ Normalized axis of grasp from endpoints as np 3-arrays """
        grasp_axis = g2 - g1
        if np.linalg.norm(grasp_axis) == 0:
            return grasp_axis
        return grasp_axis / np.linalg.norm(grasp_axis)

    @staticmethod
    def width_from_endpoints(g1, g2):
        """ Width of grasp from endpoints """
        grasp_axis = g2 - g1
        return np.linalg.norm(grasp_axis)

    # add by lifuyu compute grasp_axis by approach_method
    @property
    def approached_full_axis(self):
        """ Rotation matrix from canonical grasp reference frame to object reference frame. X axis points out of the gripper palm along the 0-degree approach direction, Y axis points between the jaws, and the Z axs is orthogonal.

        Returns
        -------
        :obj:`numpy.ndarray`
            rotation matrix of grasp
        """
        grasp_axis_y = self.axis
        grasp_axis_x = self.grasp_approach
        if np.linalg.norm(grasp_axis_x) == 0:
            grasp_axis_x = np.array([1, 0, 0])
        grasp_axis_x = grasp_axis_x / norm(grasp_axis_x)
        grasp_axis_z = np.cross(grasp_axis_x, grasp_axis_y)

        R = np.c_[grasp_axis_x, np.c_[grasp_axis_y, grasp_axis_z]]
        return R

    @property
    def T_grasp_obj(self):
        """ Rigid transformation from grasp frame to object frame.
        Rotation matrix is X-axis along approach direction, Y axis pointing between the jaws, and Z-axis orthogonal.
        Translation vector is the grasp center.

        Returns
        -------
        :obj:`RigidTransform`
            transformation from grasp to object coordinates
        """

        T_grasp_obj = RigidTransform(self.approached_full_axis, self.center, from_frame='grasp', to_frame='obj')
        return T_grasp_obj

    @property
    def T_finger_obj(self):
        """ Rigid transformation from grasp frame to object frame.
        Rotation matrix is X-axis along approach direction, Y axis pointing between the jaws, and Z-axis orthogonal.
        Translation vector is the grasp center.

        Returns
        -------
        :obj:`RigidTransform`
            transformation from grasp to object coordinates
        """
        T_finger_left_obj = RigidTransform(self.approached_full_axis, self.virtual_contactpoints[0],
                                           from_frame='left_finger', to_frame='obj')
        T_finger_right_obj = RigidTransform(self.approached_full_axis, self.virtual_contactpoints[1],
                                            from_frame='right_finger', to_frame='obj')
        return T_finger_left_obj, T_finger_right_obj


    @staticmethod
    def _get_rotation_matrix_y(theta):
        theta = theta / 180 * np.pi
        cos_t = np.cos(theta)
        sin_t = np.sin(theta)
        R = np.c_[[cos_t, 0, sin_t], np.c_[[0, 1, 0], [-sin_t, 0, cos_t]]]
        return R

    def close_fingers(self, obj, gripper, collision_manager, vis=False, check_approach=True, check_closing=True,
                      approach_dist=0.2):
        """ Steps along grasp axis to find the locations of contact with an object

        Parameters
        ----------
        obj : :obj:`GraspableObject3D`
            object to close fingers on
        vis : bool
            whether or not to plot the line of action and contact points
        check_approach : bool
            whether or not to check if the contact points can be reached
        approach_dist : float
            how far back to check the approach distance, only if checking the approach is set

        Returns
        -------
        success : bool
            whether or not contacts were found
        c1 : :obj:`Contact3D`
            the contact point for jaw 1
        c2 : :obj:`Contact3D`
            the contact point for jaw 2
        """
        # get grasp endpoints in sdf frame
        g1_world, g2_world = self.contactpoints
        # print('g1_world is : {} g2_world is : {}'.format(g1_world, g2_world))

        # check for contact along approach
        if check_approach:
            approach_axis = self.approached_full_axis[:, 0]
            z_axis = self.approached_full_axis[:, 2]
            directions = [-approach_axis] * 14

            width_ = gripper.max_width / 2 - gripper.pad - self.jaw_width_ / 2
            k = int(width_ // 0.001 + 1)
            k = k if k < 7 else 7
            for i in range(k):
                ray_origins = [g1_world - self.axis_ * (i + 0.3) * 1e-3,
                               g1_world - self.axis_ * (i + 0.3) * 1e-3 + z_axis * 0.009,
                               g1_world - self.axis_ * (i + 0.3) * 1e-3 - z_axis * 0.009,
                               g1_world - self.axis_ * (i + 0.3) * 1e-3 + z_axis * 0.006,
                               g1_world - self.axis_ * (i + 0.3) * 1e-3 - z_axis * 0.006,
                               g1_world - self.axis_ * (i + 0.3) * 1e-3 + z_axis * 0.003,
                               g1_world - self.axis_ * (i + 0.3) * 1e-3 - z_axis * 0.003,
                               g2_world + self.axis_ * (i + 0.3) * 1e-3,
                               g2_world + self.axis_ * (i + 0.3) * 1e-3 + z_axis * 0.009,
                               g2_world + self.axis_ * (i + 0.3) * 1e-3 - z_axis * 0.009,
                               g2_world + self.axis_ * (i + 0.3) * 1e-3 + z_axis * 0.006,
                               g2_world + self.axis_ * (i + 0.3) * 1e-3 - z_axis * 0.006,
                               g2_world + self.axis_ * (i + 0.3) * 1e-3 + z_axis * 0.003,
                               g2_world + self.axis_ * (i + 0.3) * 1e-3 - z_axis * 0.003]

                locations, _, _ = ApproachParallelJawPtGrasp3D.find_contact(obj, ray_origins=ray_origins,
                                                                    ray_directions=directions, vis=vis)

                if vis:
                    ray_visualize = trimesh.load_path(np.hstack((
                        np.asarray(ray_origins),
                        np.asarray(ray_origins) + np.asarray(directions))).reshape(-1, 2, 3))
                    pc_ori = trimesh.PointCloud(ray_origins, colors=[0, 255, 0])
                    scene_ray = trimesh.Scene([ray_visualize, pc_ori, obj.mesh])
                    scene_ray.show()
                approach_collision = len(locations) > 0
                closing_collision = True
                if not approach_collision:
                    # self.center_ = center + (((i+right_gain) - (i+left_gain)) / 2) * self.axis_ * 0.001
                    self.real_jaw_width_ = self.jaw_width_ + (2 * i) * 1e-3
                    if check_closing:
                        finger_left = deepcopy(gripper.finger_mesh)
                        base = deepcopy(gripper.base_mesh)
                        finger_right = deepcopy(finger_left)
                        T_base = trimesh.transformations.translation_matrix([0, 0, -0.13])
                        R_base = trimesh.transformations.euler_matrix(np.pi / 2, np.pi / 2, 0, 'rxyz')
                        base.apply_transform(T_base)
                        base.apply_transform(R_base)
                        base.apply_transform(self.T_grasp_obj.matrix)
                        T_leftfinger, T_rightfinger = self.T_finger_obj

                        T = trimesh.transformations.translation_matrix([-(gripper.pad), -0.0065, -0.048])
                        finger_left.apply_transform(T)
                        finger_right.apply_transform(T)
                        R_l = trimesh.transformations.euler_matrix(np.pi / 2, np.pi / 2, 0, 'rxyz')
                        R_r = trimesh.transformations.euler_matrix(np.pi / 2, np.pi / 2, np.pi, 'rxyz')

                        finger_left.apply_transform(R_l)
                        finger_right.apply_transform(R_r)
                        finger_left.apply_transform(T_leftfinger.matrix)
                        finger_right.apply_transform(T_rightfinger.matrix)
                        # vis = True
                        if vis:
                            scene_tmp = trimesh.Scene([finger_left, finger_right, base, obj.mesh])
                            scene_tmp.show()

                        is_collision_left = collision_manager.in_collision_single(finger_left, return_names=False,
                                                                                  return_data=False)
                        if is_collision_left:
                            # print('approach_closing collision left')
                            continue

                        is_collision_right = collision_manager.in_collision_single(finger_right, return_names=False,
                                                                                   return_data=False)

                        if is_collision_right:
                            # print('approach_closing collision right')
                            continue

                        is_collision_base = collision_manager.in_collision_single(base, return_names=False,
                                                                                  return_data=False)
                        if is_collision_base:
                            # print('approach_closing collision base')
                            continue
                        closing_collision = False
                        break

            if approach_collision or closing_collision:
                # print('approach collision')
                return False
        if vis:
            ori = self.contactpoints[0] + self.axis / 10
            target = self.contactpoints[0] - self.axis / 10
            ApproachParallelJawPtGrasp3D.show_line(ori, target, scale_factor=0.0001)
            # add by lifuyu
            mlab.points3d(self.center_[0], self.center_[1], self.center_[2], scale_factor=0.001)
            # --------------------------------
            mlab.points3d(self.virtual_contactpoints[0][0], self.virtual_contactpoints[0][1],
                          self.virtual_contactpoints[0][2], scale_factor=0.001)
            mlab.points3d(self.virtual_contactpoints[1][0], self.virtual_contactpoints[1][1],
                          self.virtual_contactpoints[1][2], scale_factor=0.001)
            mlab.triangular_mesh(obj.mesh.vertices[:, 0], obj.mesh.vertices[:, 1], obj.mesh.vertices[:, 2],
                                 obj.mesh.faces, representation='surface', color=(0.5, 0.5, 0), opacity=1.0)
            mlab.triangular_mesh(finger_left.vertices[:, 0], finger_left.vertices[:, 1], finger_left.vertices[:, 2],
                                 finger_left.faces, representation='surface', color=(0, 0.5, 0.5), opacity=1.0)
            mlab.triangular_mesh(finger_right.vertices[:, 0], finger_right.vertices[:, 1], finger_right.vertices[:, 2],
                                 finger_right.faces, representation='surface', color=(0, 0.5, 0.5), opacity=1.0)

            mlab.show()
            pass

        contacts_found = True
        return contacts_found

    @staticmethod
    def create_line_of_action(g, axis, width, obj, num_samples, min_width=0, convert_grid=True):
        """
        Creates a straight line of action, or list of grid points, from a given point and direction in world or grid coords

        Parameters
        ----------
        g : 3x1 :obj:`numpy.ndarray`
            start point to create the line of action
        axis : normalized 3x1 :obj:`numpy.ndarray`
            normalized numpy 3 array of grasp direction
        width : float
            the grasp width
        num_samples : int
            number of discrete points along the line of action
        convert_grid : bool
            whether or not the points are specified in world coords

        Returns
        -------
        line_of_action : :obj:`list` of 3x1 :obj:`numpy.ndarrays`
            coordinates to pass through in 3D space for contact checking
        """
        num_samples = max(num_samples, 3)  # always at least 3 samples
        line_of_action = [g + t * axis for t in
                          np.linspace(0, float(width) / 2 - float(min_width) / 2, num=num_samples)]
        if convert_grid:
            as_array = np.array(line_of_action).T
            transformed = obj.sdf.transform_pt_obj_to_grid(as_array)
            line_of_action = list(transformed.T)
        return line_of_action

    @staticmethod
    def find_contact(obj, ray_origins, ray_directions, vis=True):
        locations, index_ray, index_tri = obj.mesh.ray.intersects_location(ray_origins, ray_directions,
                                                                           multiple_hits=True)
        return locations, index_ray, index_tri

    @staticmethod
    def appoach_find_contact(obj, grasp_axis,grasp_point, direction, circle_sampled_points_num, radius, vis=False):
        contact_points=[]
        contact_points_index_tri=[]
        lines = trimesh.intersections.mesh_plane(obj.mesh, direction, grasp_point, return_faces=False, cached_dots=None)

        if lines.shape[0] == 0:
            return [],[]

        grasp_points = np.array([grasp_point] * 2)
        grasp_axis_ =  np.concatenate((grasp_axis,-grasp_axis),axis=0).reshape(2,3)
        # print(grasp_axis_)
        locations, index_ray, index_tri = obj.mesh.ray.intersects_location(grasp_points,
                                                                           grasp_axis_,
                                                                            multiple_hits=True)
        # print(locations, index_ray, index_tri)
        if (0 in index_ray) and (1 in index_ray):
            index_0 = np.where(index_ray == 0)
            index_1 = np.where(index_ray == 1)

            distance_0 = np.linalg.norm(locations[index_0] - grasp_point, axis=1)
            distance_1 = np.linalg.norm(locations[index_1] - grasp_point, axis=1)

            ind_0 = np.where(np.logical_and(distance_0 > 0.0005, distance_0 < 0.0225))
            ind_1 = np.where(np.logical_and(distance_1 > 0.0005, distance_1 < 0.0225))

            a = np.array(ind_0).reshape(-1)
            b = np.array(ind_1).reshape(-1)
            if (a.shape[0] != 0) and (b.shape[0] != 0):
                ind_0_max = np.argmax(distance_0[ind_0])
                ind_1_max = np.argmax(distance_1[ind_1])
                c1 = locations[index_0][ind_0][ind_0_max]
                c2 = locations[index_1][ind_1][ind_1_max]
                index_tri0 = index_tri[index_0][ind_0][ind_0_max]
                index_tri1 = index_tri[index_1][ind_1][ind_1_max]
                # print(ind_0_max, ind_1_max)
                # print('c',c1,c2)
                contact_points.append( np.asarray([c1, c2]).reshape(2,3))
                # print(contact_points)
                contact_points_index_tri.append(np.asarray([index_tri0, index_tri1]))
            else:
                pass
        else:
            pass
            # print('no contact_points')

        vis = False
        if vis:
            pc_locations = trimesh.PointCloud(locations,colors=[0,0,255])
            pc_grasp_point = trimesh.PointCloud(grasp_point.reshape(1,3),colors=[255,0,0])
            ray_axis = trimesh.load_path(np.hstack((
                grasp_points,
                grasp_points + grasp_axis_ / 20)).reshape(-1, 2, 3))
            ray_approach = trimesh.load_path(np.hstack((
                grasp_point,
                grasp_point + direction / 20)).reshape(-1, 2, 3))
            ray_normal = trimesh.load_path(np.hstack((
                grasp_point,
                grasp_point - direction / 20)).reshape(-1, 2, 3))
            change_ray_color(ray_approach,[0,0,255])
            change_ray_color(ray_normal,[255,0,255])
            if len(contact_points) !=0:
                pc_contact_points=trimesh.PointCloud(contact_points[0],colors=[0, 255, 0])
                scene = trimesh.Scene([pc_grasp_point, ray_axis, obj.mesh, ray_approach, ray_normal])
                scene.add_geometry(pc_contact_points)
                scene.show()
            else:
                pass

        return contact_points,contact_points_index_tri

    @staticmethod
    def appoach_find_grasp_axis(obj, grasp_point, direction, circle_sampled_points_num, radius, vis=False):
        vertices = trimesh.creation.cylinder(radius=radius, height=2, sections=circle_sampled_points_num,
                                             transform=trimesh.transformations.translation_matrix([0, 0, 1])).vertices
        predefined_vertices = vertices[vertices[:, 2] == 0]
        R = tf.rotation_matrix(tf.angle_between_vectors(direction, np.array([0, 0, 1])),
                               tf.vector_product(direction, np.array([0, 0, 1])))

        circle_vertices = np.dot(predefined_vertices, R[:3, :3]) + grasp_point

        vector_list = []
        for k in range(20):
            if (k == 0) or (k==2):
                continue
            vector = circle_vertices[k, :] - circle_vertices[0, :]
            vector = vector / np.linalg.norm(vector)
            vector_list.append(vector)

        grasp_axis = np.asarray(vector_list)

        vis = False
        if vis:
            originpoints = np.array([grasp_point] * 18)
            ray = trimesh.load_path(np.hstack((
                originpoints,
                originpoints + grasp_axis / 20)).reshape(-1, 2, 3))
            ray_approach = trimesh.load_path(np.hstack((
                grasp_point,
                grasp_point - direction / 20)).reshape(-1, 2, 3))
            change_ray_color(ray_approach, [0, 0, 255])
            scene = trimesh.Scene([ray,ray_approach])
            scene.show()

        return grasp_axis

    @staticmethod
    def grasp_from_point_and_axis_on_mesh(gripper, obj, sampled_point, grasp_approach, grasp_width_world,
                                          friction_coef=None, collision_manager=None, vis=False):

        # transform to grid basis
        grasp_candidate_world = sampled_point.point  # sampled grasp point
        grasp_approach = grasp_approach / np.linalg.norm(grasp_approach)

        # print(grasp_approach,grasp_approach.shape)
        # backup point along the normal

        grasps = []
        distance_pre = 1
        contact_found_list = []
        backup_ = -0.009
        point_backup_= deepcopy(grasp_candidate_world)
        point_backup_+= backup_ * grasp_approach
        grasp_axis = ApproachParallelJawPtGrasp3D.appoach_find_grasp_axis(obj, grasp_point=point_backup_,
                                                                          direction=grasp_approach,
                                                                          circle_sampled_points_num=36, radius=0.0225,
                                                                          vis=vis)
        for axis in grasp_axis:
            for num_step in range(16):
                onward = num_step * 0.002
                point_onward = deepcopy(point_backup_)
                point_onward += onward * grasp_approach
                contact_points, contact_points_index_tri = ApproachParallelJawPtGrasp3D.appoach_find_contact(obj,axis,grasp_point=point_onward,direction=grasp_approach,
                                                                           circle_sampled_points_num=36,radius=0.0225,vis=vis)
                if len(contact_points) == 0:  # means no contact points no grasp
                    continue

                distance_0 = np.linalg.norm((contact_points[0][0, :] - point_onward))
                distance_1 = np.linalg.norm((contact_points[0][1, :] - point_onward))
                distance_ = 2 * max(distance_0, distance_1)

                if gripper.max_width - 2 * gripper.pad > distance_ > gripper.min_width:
                    c1_ = Contact3D(obj, index=contact_points_index_tri[0][0], contact_point=contact_points[0][0, :])  # contact points 1
                    c2_ = Contact3D(obj, index=contact_points_index_tri[0][1], contact_point=contact_points[0][1, :])  # contact points 2
                    force_closure_score = PointGraspMetrics3D.force_closure(c1_, c2_, friction_coef)
                    # print(force_closure_score)
                    if force_closure_score > 0:
                        if distance_pre - distance_ > gripper.pad + gripper.finger_tip_width:
                            # compute grasp center Offset
                            distance_arr = np.array([distance_0, distance_1])
                            ind = np.unravel_index(np.argmax(distance_arr, axis=None), distance_arr.shape)
                            axis_direction = np.array([axis, -axis])
                            max_direction = axis_direction[ind]
                            l2_contact_points =  np.linalg.norm((contact_points[0][0, :] - contact_points[0][1, :]))
                            offset_distance = l2_contact_points / 2 - min(distance_0, distance_1)
                            if ind[0] == 0:
                                offset_distance *=1
                            else:
                                offset_distance *=-1
                            grasp_center_offset = deepcopy(point_onward) + offset_distance * axis
                            grasp_width_new = l2_contact_points

                            vis = False
                            if vis:
                                pc_1 = trimesh.PointCloud(grasp_candidate_world.reshape(1, 3), colors=[0, 255, 0])
                                pc_2 = trimesh.PointCloud(point_onward.reshape(1,3), colors=[255, 255, 0])
                                pc_3 = trimesh.PointCloud(contact_points[0],colors=[0, 0, 255])
                                pc_4 = trimesh.PointCloud(grasp_center_offset.reshape(1, 3), colors=[255, 0, 255])
                                ray_approach = trimesh.load_path(np.hstack((
                                    grasp_candidate_world,
                                    grasp_candidate_world + grasp_approach / 20)).reshape(-1, 2, 3))
                                ray_axis = trimesh.load_path(np.hstack((
                                    point_onward.reshape(1, 3),
                                    point_onward.reshape(1, 3) + axis / 20)).reshape(-1, 2, 3))
                                ray_axis_ = trimesh.load_path(np.hstack((
                                    point_onward.reshape(1, 3),
                                    point_onward.reshape(1, 3) - axis / 20)).reshape(-1, 2, 3))
                                change_ray_color(ray_approach, [0, 0, 255])
                                change_ray_color(ray_axis, [255, 0, 0])
                                change_ray_color(ray_axis_, [255, 0, 0])
                                scene_ = trimesh.Scene([pc_1, pc_2, pc_4, ray_approach, ray_axis, obj.mesh,pc_3,ray_axis_])
                                scene_.show()
                            grasp_center = deepcopy(point_onward)
                            grasp_point = deepcopy(grasp_candidate_world) # grasp point is sampled point
                            d = deepcopy(backup_) + onward
                            grasp_approach_distance = d
                            grasp_axis = axis
                            jaw_width_world = distance_
                            # print(jaw_width_world)
                            grasp_force_closure_score = force_closure_score * 0.50
                            value = 1 - np.linalg.norm(grasp_center) / 0.03
                            value = value if value > -3 else -3
                            grasp_center_score = value * 0.20
                            # grasp_width_score = (jaw_width_world // ((gripper.max_width - 2 * gripper.pad)/4) + 1)/4 * 0.15
                            grasp_curvature_score = c1_.curvature_score * c2_.curvature_score * 0.30
                            grasp_score_sum = grasp_force_closure_score + grasp_center_score + grasp_curvature_score
                            grasp_score = [grasp_force_closure_score, grasp_center_score, grasp_curvature_score]
                            if grasp_score_sum <= 0.0:
                                continue
                            # if jaw_width_world > 0.01:
                            #     continue
                            # print('grasp_width_new',grasp_width_new)
                            configuration = ApproachParallelJawPtGrasp3D.approach_configuration_from_params(grasp_center,
                                                                                                    grasp_axis,
                                                                                                    grasp_width_world,
                                                                                                    grasp_approach,
                                                                                                    jaw_width_world,
                                                                                                    grasp_score,
                                                                                                    grasp_approach_distance,
                                                                                                    grasp_point,
                                                                                                    offset_distance,
                                                                                                    grasp_width_new)
                            grasp = ApproachParallelJawPtGrasp3D(configuration)
                            success = grasp.close_fingers(obj, gripper, collision_manager, vis=vis)
                            if success:
                                contact_found_list.append(1)
                                grasps.append(grasp)
                            else:
                                break
                    else:
                        pass
                else:
                    pass

        if 1 in contact_found_list:
            contact_found = True
        else:
            contact_found = False

        if not contact_found:
            logging.debug('No contacts found for grasp')
            return False, []
        return contact_found, grasps




